﻿#ifndef MECVMATH_H
#define MECVMATH_H

#include "MePoint.h"

#include <opencv2/core.hpp>

#include "matrixd.h"

/// opencv 常用的函数集成
class MeCvMath
{
public:
    template<typename _Tp>
    static double pow2(cv::Point3_<_Tp> pt) {
        return pt.x*pt.x + pt.y*pt.y + pt.z*pt.z;
    }
    template<typename _Tp>
    static double distanceFrom2p(cv::Point3_<_Tp> a, cv::Point3_<_Tp> b) {
        return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2) + pow(a.z - b.z, 2));
    }

    template<typename _Tp>
    static double distance(cv::Point_<_Tp> &a, cv::Point_<_Tp> &b) {
        return  sqrt(pow(a.x - b.x, 2) + pow(a.y -b.y, 2));
    }

    template<typename _Tp>
    static double distance3D(cv::Point3_<_Tp> &a, cv::Point3_<_Tp> &b) {
        return  sqrt(pow(a.x - b.x, 2) + pow(a.y -b.y, 2) + pow(a.z -b.z, 2));
    }

    /// 转成point3f/ point3d
    template<typename _Tp>
    static cv::Point3_<_Tp> cvPoint3FromPoint(_Tp v[3]) {
        return cv::Point3_<_Tp>(v[0], v[1], v[2]);
    }
    /// 转成point3f/ point3d
    template<typename _Tp>
    static void cvPoint3ToPoint(cv::Point3_<_Tp> pt, _Tp v[3]) {
        v[0] = pt.x;
        v[1] = pt.y;
        v[2] = pt.z;
    }
    template<typename _Tp>
    static void cvPoint3ToPoint(cv::Point3_<_Tp> pt, MePoint3D &mePt) {
        mePt.v[0] = pt.x;
        mePt.v[1] = pt.y;
        mePt.v[2] = pt.z;
    }

    static std::vector<cv::Point3d> toCvPoints(std::vector<MePoint3D> &pts) {
        std::vector<cv::Point3d> p3ds;
        for(MePoint3D &p: pts) {
            p3ds.push_back({p[0], p[1], p[2]});
        }
        return p3ds;
    }

    static std::vector<cv::Point2f> keyPointsToCvPoints(std::vector<cv::KeyPoint> &kps)
    {
        std::vector<cv::Point2f> points;
        for (uint i = 0; i < kps.size(); i++) {
            points.push_back(kps[i].pt);
        }
        return points;
    }

    static void keyPointsToCvPoints(std::vector<cv::KeyPoint> &kps, std::vector<cv::Point2f> &outPts)
    {
        outPts.clear();
        for (uint i = 0; i < kps.size(); i++) {
            outPts.push_back(kps[i].pt);
        }
    }

    static void keyPointsFromCvPoints(std::vector<cv::Point2f> &inPts, float diameter, std::vector<cv::KeyPoint> &outKps)
    {
        outKps.clear();
        for (uint i = 0; i < inPts.size(); i++) {
            outKps.push_back({inPts[i], diameter});
        }
    }

    static cv::Mat cvPoint3fToMat3x1d(const cv::Point3f &point3d) {
        cv::Mat mat = cv::Mat(3, 1, CV_64FC1);
        mat.at<double>(0) = point3d.x;
        mat.at<double>(1) = point3d.y;
        mat.at<double>(2) = point3d.z;

        return mat;
    }

    static cv::Point3f cvMat3x1dToPoint3f(const cv::Mat &mat) {
        cv::Point3f p;
        p.x = mat.at<double>(0);
        p.y = mat.at<double>(1);
        p.z = mat.at<double>(2);

        return p;
    }

    /// 计算点的均值和距离 stddev
    static int meanStdDevPoints(const std::vector<cv::Point3d> &points, cv::Point3d &meanP, double &mean, double &stddev,
                                double &maxV);
    /// 齐次矩阵和vector相乘
    static void multiplyPoint3f(Matrix4d &m, const cv::Point3f &vec, cv::Point3f &outP);

    /// R: 3x3 to 4x4
    static Matrix4d toRotMatrix4d(cv::Mat &R);
    /// m = [R | t]
    ///      0 | 1
    static Matrix4d toMatrix4d(cv::Mat &R, cv::Mat &t);
};

#endif // MECVMATH_H
